
#include <BPainter>
#include <BRender>
#include <BViewer>
#include <BScene>
#include <BPhysics>
#include <BCone>
#include <BCube>
#include <BImage>
#include <BMaterial>
#include <BReadWrite>

#include <btConeTwistConstraint.h>
#include "ConearHandler.h"

ConearHandler::ConearHandler()
{
	_lengthText = new BTextual();
	_lengthText->setColor(200, 0, 0);
	_lengthText->setCullback(false);
	_lengthText->setLighting(false);
	_lengthText->setPolyMode(Poly_Fill);
	_lengthText->setGlyphSize(0.3);

	BImage* image = ReadFile<BImage>("data/image/whitemetal_diffuse.jpg");

	_nodeA = new BNode("nodeA");
	{
		BPhysics* physics = new BPhysics();
		physics->setMass(0);
		_nodeA->setPhysics(physics);

		BCube* cube = new BCube(4, 12);
		cube->setColor(100, 200, 255);
		_nodeA->addShape(cube);

	}

	_nodeB = new BNode("nodeB");
	{
		BPhysics* physics = new BPhysics();
		physics->setMass(1.0);
		_nodeB->setPhysics(physics);

		BReal down = 2 * 0.25f;
		BCone* cone = new BCone(0.5, 2, 24, 10);
		cone->flush();
		BVectorArray* vertices = cone->vertices();
		for (int i = 0; i < vertices->size(); i++)
		{
			vertices->at(i).z() += down;
		}
		cone->setTexture(image);
		cone->setColor(200, 100, 0);
		_nodeB->addShape(cone);
	}

	BVector pointA = _nodeA->space().max();
	BVector axisA(1, 0, 0);
	BVector pointB(-5, 0, _nodeB->space().max().z());
	BVector axisB(1, 0, 0);
	_constraint = new BConearConstraint(_nodeA, pointA, _nodeB, pointB);
	_constraint->setSwingAxis(BVector(0, 0, 1));
	_constraint->setInnerCollible(false);
	_constraint->setName("Conear");
}
ConearHandler::~ConearHandler()
{

}

void ConearHandler::attached(BViewer* viewer)
{
	BScene* dscene = viewer->scene();
	_nodeA->setPosition(0, 0, 5);
	_nodeB->setPosition(2, 2, 5);
	dscene->insert(_nodeA);
	dscene->insert(_nodeB);
	dscene->insert(_constraint);
	_lengthText->setFont(viewer->font());
	viewer->scene()->insert(_lengthText);
	this->emit("edit-constraint", _constraint);
}
void ConearHandler::detached(BViewer* viewer)
{
	BScene* dscene = viewer->scene();
	dscene->remove(_nodeA);
	dscene->remove(_nodeB);
	dscene->remove(_constraint);
	viewer->scene()->remove(_lengthText);
}

void ConearHandler::paint(BPainter& painter)
{
	BViewer* viewer = this->viewer();
	painter.setLineWidth(10);
	painter.setColor(BColor(0, 1, 0));
	painter.drawRect(10, 10, viewer->width() - 20, viewer->height() - 20);
}
void ConearHandler::render(BRender& render)
{
	render.setLighting(false);
	render.setDepthTest(false);
	render.setLineSmooth(true);
	render.setLineWidth(1);

	double length1 = _constraint->nodeA()->space().radius() * 2;
	btConstraint* typedConstraint = _constraint->asset();
	if (btConeTwistConstraint* pCT = dynamic_cast<btConeTwistConstraint*>(typedConstraint))
	{
		btTransform transformA = pCT->getRigidBodyA()->getTransform() * pCT->getAFrame();
		{
			btVector3 origin = transformA.getOrigin();
			btVector3 axisA = origin + transformA.getBasis() * btVector3(length1, 0, 0);
			btVector3 axisB = origin + transformA.getBasis() * btVector3(0, length1, 0);
			btVector3 axisC = origin + transformA.getBasis() * btVector3(0, 0, length1);
			BVector start(origin.x(), origin.y(), origin.z());
			BVector pointA(axisA.x(), axisA.y(), axisA.z());
			BVector pointB(axisB.x(), axisB.y(), axisB.z());
			BVector pointC(axisC.x(), axisC.y(), axisC.z());
			render.setColor(255, 66, 66);
			render.drawLine(start, pointA);
			render.setColor(66, 255, 66);
			render.drawLine(start, pointB);
			render.setColor(66, 66, 255);
			render.drawLine(start, pointC);
		}

		static int nSegments = 8 * 4;
		BReal fAngleInRadians = BReal(2. * 3.1415926) * (BReal)(nSegments - 1) / BReal(nSegments);
		btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length1);
		pPrev = transformA * pPrev;
		render.setColor(220, 240, 255);
		for (int i = 0; i < nSegments; i++)
		{
			fAngleInRadians = BReal(2. * 3.1415926) * (BReal)i / BReal(nSegments);
			btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length1);
			pCur = transformA * pCur;
			render.drawLine(pPrev.x(), pPrev.y(), pPrev.z(), pCur.x(), pCur.y(), pCur.z());

			if (i % (nSegments / 8) == 0)
			{
				render.drawLine(pPrev.x(), pPrev.y(), pPrev.z(), pCur.x(), pCur.y(), pCur.z());
			}
			pPrev = pCur;
		}

		const BReal length2 = _constraint->nodeB()->space().radius() * 2;
		btTransform transformB = pCT->getRigidBodyB()->getTransform() * pCT->getBFrame();
		{
			btVector3 origin = transformB.getOrigin();
			btVector3 axisA = origin + transformB.getBasis() * btVector3(length2, 0, 0);
			btVector3 axisB = origin + transformB.getBasis() * btVector3(0, length2, 0);
			btVector3 axisC = origin + transformB.getBasis() * btVector3(0, 0, length2);
			BVector start(origin.x(), origin.y(), origin.z());
			BVector pointA(axisA.x(), axisA.y(), axisA.z());
			BVector pointB(axisB.x(), axisB.y(), axisB.z());
			BVector pointC(axisC.x(), axisC.y(), axisC.z());
			render.setColor(255, 66, 66);
			render.drawLine(start, pointA);
			render.setColor(66, 255, 66);
			render.drawLine(start, pointB);
			render.setColor(66, 66, 255);
			render.drawLine(start, pointC);
		}

		BReal twSpan = pCT->getTwistSpan();
		BReal twAngle = pCT->getTwistAngle();
		bool useFrameB = (pCT->getRigidBodyB()->getInvMass() > BReal(0.f));
		if (useFrameB)
		{
			transformB = pCT->getRigidBodyB()->getTransform() * pCT->getBFrame();
		}
		else
		{
			transformB = pCT->getRigidBodyA()->getTransform() * pCT->getAFrame();
		}
		btVector3 pivot = transformB.getOrigin();
		btVector3 normal = transformB.getBasis().getColumn(0);
		btVector3 axis1 = transformB.getBasis().getColumn(1);
		render.setColor(22, 44, 66);
		//render.drawArc(pivot, normal, axis1, length, length, -twAngle - twSpan, -twAngle + twSpan);
	}
}
